/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#ifndef _COccuMCL__H_
#define _COccuMCL__H_

#include "CParticleFilter.h"
#include "TOccupancyGrid.h"

#define VMAP 0.1					// Map uncertainty
#define VANG 0.1					// Angle uncertainty
#define VR	 0.02					// Uncertainty of the range measurement
#define FALSE_PROB 0.5				// "Probability" of the false measurement
#define VMEAS ((VMAP+VANG+VR)*(VMAP+VANG+VR)) // Whole measurement uncertainty

class COccuMCL{
		public:
				CParticleFilter filt; ///<the particle filter used
				TOccupancyGrid *O;
				float prob_var; ///< The variance of probability distribution
				float prob_max; ///< The probability of maximum likelihood particle
				COccuMCL();
				~COccuMCL();
				
				/**
				 * Initializes the filter - uses normal random method for calculating the initial distribution
				 * Loads the map from line file, reserves memory for particles, sets the initial distribution
				 * @p *mapfile The mapfile name
				 * @p num_of_particles The number of particles in the filter
				 * @p init_pose the initial pose
				 * @p initVariance the variance for the initial quess
				 * @p num_of_meas_in_scan number of measurements used from the scan
				 * @p occu_val The value that is threshold for determine if the cell is occupied
				 */
				void initialize(const char *mapfile,
												int num_of_particles, 
												mcl::pose initPose, 
												mcl::pose initVariance,
												int num_of_meas_in_scan,
												float _sensor_offset,
												grid_t occu_val);
				
			/**
				 * Initializes the filter - uses normal random method for calculating the initial distribution
				 * Loads the map from line file, reserves memory for particles, sets the initial distribution
				 * @p *mapfile The mapfile name
				 * @p mapscale the scale factor needed to get the map coordinates into meters
				 * @p num_of_particles The number of particles in the filter
				 * @p init_pose the initial pose
				 * @p initVariance the variance for the initial quess
				 * @p num_of_meas_in_scan number of measurements used from the scan
				 * @p resolution The resolution of the occupancy grid (in meters)
				 * @p lineval The value the line map is set to in grid
			 */
				void initialize(const char *mapfile,
																	float mapscale, 
																	int num_of_particles, 
																	mcl::pose initPose, 
																	mcl::pose initVariance,
																	int num_of_meas_in_scan,
																	float _sensor_offset,
																	float resolution,
																	grid_t lineval);
				
				/**
				 * Initializes the filter - uses uniform distribution method for calculating the initial distribution
				 * Loads the map from file, reserves memory for particles, sets the initial distribution
				 * The initial distribution is calculated using bounding box of the map. The @dP sets the 
				 * resolution of the unform grid. WARNING the initialization might not be 
				 * real-time.
				 * @p *mapfile The mapfile name
				 * @p mapscale the scale factor needed to get the map coordinates into meters
				 * @p dP the resolution of the uniform grid
				 * @p num_of_meas_in_scan number of measurements used from the scan
				 */
				void initializeGlobal(const char *mapfile,
												float mapscale, 
												mcl::pose dP, 
												int num_of_meas_in_scan,
												float _sensor_offset);

				/**
				* Same as init but only for the filter
				* Initializes the filter - uses normal random method for calculating the initial distribution
				*/
				void reinitializeNR(mcl::pose initPose,mcl::pose initVariance);
				
				/**
				* Run the MCL step
				* @p dP the estimated relative movement between steps (prediction)
				* @p variance the error of prediction
				* @p measurement the scan taken from new position
				*/
				void runMCL(mcl::pose dP, mcl::pose variance, mcl::scan measurement);
				
				/**
				* Run the MCL step in initialization mode
				* @p dP the estimated relative movement between steps (prediction)
				* @p variance the error of prediction
				* @p measurement the scan taken from new position
				*/
				void runMCLInit(mcl::pose dP, mcl::pose variance, mcl::scan measurement);
				
				void getScan(mcl::scan &meas, mcl::pose &p);
				void updateDt();
				void setOccupiedValue(grid_t value){
						o_val = value;
				}
		private:
				
				float *dist;
				Grid::TScan virt;
				int num_of_meas;
				float sensor_offset;
				grid_t o_val; ///< the value for occupied cell
				float measurement_variance;
				float vmeas;
				mcl::pose last_meas_update;
				/**
				* Calculates the likelihood of particle distribution
				* uses the CParticleFilter filt and TLineMap map
				* @param meas The current measurement
				**/
				void calculateLikelihood(mcl::scan meas);
				
				/**
				* Initialises the variables
				*/
				void initializeVariables(const char *mapfile,
																 float mapscale,  
																 int num_of_meas_in_scan,
																 float _sensor_offset,
																 float resolution,
																 grid_t lineval);
};

#endif


